#include "./delivery.h"
#include "./robot_arm.h"
#include "../driver/fridge.h"
#include "../driver/tube.h"
#include "../driver/ctrl.h"
#include "../driver/robot_arm1.h"
#include "../driver/robot_arm2.h"
#include "./device_info.h"
#include "../driver/sensor.h"

namespace service::delivery {

using namespace driver;
using namespace utils;

static bool m_pre_proce_flag = false;

void pre_proce(const message_t &msg) {
    m_pre_proce_flag = true;
    msg.write(proto::Preprocess, ec::Ok);
}

void begin(const message_t &msg) {
    if (!m_pre_proce_flag) {
        return;
    }
    m_pre_proce_flag = false;

    error_t err = fridge->open();
    msg.nofity_step(prog::STEP1, err);
    if (err) {
        goto finish;
    }
finish:
    msg.write(proto::Begin, err);
}

static error_t pick_up(const message_t &msg) {
    uint16_t p1, p2, depth;
    deviceInfo->get_pick_info(p1, p2, depth);

    error_t err = robotArm2->run(p2);
    msg.nofity_step(prog::STEP10, err);
    if (err) {
        goto finish;
    }
    err = robotArm1->run(p1);
    msg.nofity_step(prog::STEP11, err);
    if (err) {
        goto finish;
    }
    if (!sensor->is_compr()) {
        ctrl::compr_off();
        err = ec::DropOut;
        goto finish;
    }
    err = tube->down(depth);
    msg.nofity_step(prog::STEP12, err);
    if (err) {
        goto finish;
    }
    ctrl::compr_off();
    osDelay(5 * 1000);
finish: 
    return err;
}

static error_t run_cargo(const message_t &msg, const cargo_t &cargo) {
    error_t err = tube->reset();
    msg.nofity_step(prog::STEP2, err);
    if (err) {
        goto finish;
    }

    err = robotArm1->reset();
    msg.nofity_step(prog::STEP3, err);
    if (err) {
        goto finish;
    }
    
    err = robotArm2->run(cargo.p2);
    msg.nofity_step(prog::STEP4, err);
    if (err) {
        goto finish;
    }

    err = robotArm1->run(cargo.p1);
    msg.nofity_step(prog::STEP5, err);
    if (err) {
        goto finish;
    }

    if (cargo.depth1 > 0) {
        err = tube->down(cargo.depth1);
        msg.nofity_step(prog::STEP6, err);
        if (err) {
            goto finish;
        }
    }

    err = tube->draw(cargo.depth2);
    msg.nofity_step(prog::STEP7, err);
    if (err) {
        goto finish;
    }

    err = tube->reset();
    msg.nofity_step(prog::STEP8, err);
    if (err) {
        goto finish;
    }

    if (!sensor->is_compr()) {
        ctrl::compr_off();
        err = ec::DropOut;
        goto finish;
    }

    err = robotArm1->reset();
    msg.nofity_step(prog::STEP9, err);
    if (err) {
        goto finish;
    }

    err = pick_up(msg);
    if (err) {
        goto finish;
    }

finish:
    return err;
}

void run(const message_t &msg) {
    if (!m_pre_proce_flag) {
        return;
    }
    m_pre_proce_flag = false;
    
    error_t err;
    uint8_t num;
    cargo_t cargo;

    err = msg.parse(cargo.row, cargo.col, num);
    if (err) {
        goto finish;
    }

    err = deviceInfo->get_by_cargo(cargo);
    if (err) {
        goto finish;
    }
    
    err = fridge->open();
    msg.nofity_step(prog::STEP1, err);
    if (err) {
        goto finish;
    }
    for (uint8_t i = 0; i < num; i ++) {
        err = run_cargo(msg, cargo);
        if (err == ec::DrawFail) {
            continue;
        }
        break;
    }
    if (err) {
        goto finish;
    }
finish:
    msg.write(proto::Run, err);
}

static error_t reset_inner(const message_t &msg) {
    ctrl::compr_off();

    error_t err = tube->reset();
    msg.nofity_step(prog::STEP13, err);
    if (err) {
        goto finish;
    }

    err = fridge->close();
    msg.nofity_step(prog::STEP14, err);
    if (err) {
        goto finish;
    }

    err = robotArm1->reset();
    msg.nofity_step(prog::STEP15, err);
    if (err) {
        goto finish;
    }

    err = robotArm2->reset();
    msg.nofity_step(prog::STEP16, err);
    if (err) {
        goto finish;
    }

finish:
    return err;
}

void finish(const message_t &msg) {
    if (!m_pre_proce_flag) {
        return;
    }
    m_pre_proce_flag = false;

    error_t err;
    err = reset_inner(msg);
finish:
    msg.write(proto::Finish, err);
}

void reset(const message_t &msg) {

    error_t err = reset_inner(msg);
finish:
    msg.write(proto::Reset, err);
}

void reset() {
    tube->reset();
    fridge->close();
}

void cargo_exec(const message_t &msg) {
    if (!m_pre_proce_flag) {
        return;
    }
    m_pre_proce_flag = false;

    cargo_t cargo;
    uint8_t num;
    error_t err = msg.parse(cargo.row, cargo.col, num);
    if (err) {
        goto finish;
    }

    err = deviceInfo->get_by_cargo(cargo);
    if (err) {
        goto finish;
    }

    err = fridge->open();
    if (err) {
        goto finish;
    }

    for (uint8_t i = 0; i < num; i ++) {
        err = run_cargo(msg, cargo);
        if (err == ec::DrawFail) {
            continue;
        }
        break;
    }

    err = reset_inner(msg);
    if (err) {
        goto finish;
    }

finish:
    msg.write(proto::CargoDelivery, err);
}

error_t cargo_test(cargo_t &cargo) {
    error_t err = tube->reset();
    if (err) {
        goto finish;
    }
    err = robotArm1->reset();
    if (err) {
        goto finish;
    }
    err = robotArm2->reset();
    if (err) {
        goto finish;
    }
    err = deviceInfo->get_by_cargo(cargo);
    if (err) {
        goto finish;
    }
    err = robotArm2->run(cargo.p2);
    if (err) {
        goto finish;
    }
    err = robotArm1->run(cargo.p1);
    if (err) {
        goto finish;
    }
    err = tube->down(cargo.depth1);
finish:
    return err;
}

}

